#include "ros/ros.h"
#include "nav_msgs/Odometry.h"

// 回调函数，用于接收并显示线速度和角速度
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    // 提取线速度和角速度
    double linear_velocity = msg->twist.twist.linear.x;  // 线速度
    double angular_velocity = msg->twist.twist.angular.z;  // 角速度

    // 打印到命令行窗口
    ROS_INFO("Linear Velocity: %.2f m/s, Angular Velocity: %.2f rad/s", linear_velocity, angular_velocity);
}

int main(int argc, char** argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "display_speed_node");

    // 创建节点句柄
    ros::NodeHandle nh;

    // 订阅 /odom 话题
    ros::Subscriber odom_sub = nh.subscribe("/odom", 10, odomCallback);

    // 循环等待回调
    ros::spin();

    return 0;
}